#include <assert.h>
#include "cubic_bspline_curve.h"
#include "cubic_bspline_curve_cuda.h"


namespace shermit {

#define HCurve BSplineCurve<Float_t, 3, DeviceType::CPU>

	template<typename Float_t>
	HCurve::HCurve() {
		control_pts.reserve(degree() + 1);  // a p-degree bspline curve requires at least p+1 control pts
	}

	template<typename Float_t>
	inline int HCurve::degree() const {
		return 3;
	}

	template<typename Float_t>
	inline int HCurve::nControlPts() const {
		return control_pts.size();
	}

	template<typename Float_t>
	inline int HCurve::nKnots() const {
		return nControlPts() + degree() + 1;
	}

	template<typename Float_t>
	const nv::vec2<Float_t>* HCurve::getControlPts() const {
		return control_pts.data();
	}

	template<typename Float_t>
	void HCurve::addControlPt(const HCurve::vec_t& pt) {
		control_pts.push_back(pt);
	}

	template<typename Float_t>
	void HCurve::setControlPt(int i, HCurve::vec_t& pt) {
		control_pts[i] = pt;
	}

	template<typename Float_t>
	void HCurve::setControlPts(const HCurve::vec_t* pts, int num_pts, DeviceType from) {
		if (!pts || num_pts <= 0) return;
		control_pts.resize(num_pts);
		if (from == DeviceType::CPU) {
			memcpy(&control_pts[0], pts, sizeof(vec_t) * num_pts);
		}
#if enable_cuda
		else {
			checkCudaErrors(GPU2CPU(&control_pts[0], pts, sizeof(vec_t) * num_pts));
		}
#endif
		return;
	}

	template<typename Float_t>
	inline void HCurve::deallocate() {
		control_pts.clear();
	}
	
	template<typename Float_t>
	inline Float_t HCurve::tMin() const {
		return (Float_t)degree();
	}

	template<typename Float_t>
	inline Float_t HCurve::tMax() const {
		return (Float_t)nControlPts();
	}

	template<typename Float_t>
	nv::vec2<Float_t> HCurve::at(Float_t t) const {
		assert(nControlPts() >= degree() + 1);
		assert(t >= tMin());
		assert(t < tMax());

		int k = t;
		Float_t t0 = t - (k - degree());
		Float_t it0 = 4 - t0;
		Float_t b0 = it0 * it0 * it0 / 6;
		Float_t t1 = t0 - 1;
		Float_t it1 = 4 - t1;
		Float_t b1 = (-3 * it1 * it1 * it1 + 12 * it1 * it1 - 12 * it1 + 4) / 6;
		Float_t t2 = t1 - 1;
		Float_t b2 = (-3 * t2 * t2 * t2 + 12 * t2 * t2 - 12 * t2 + 4) / 6;
		Float_t t3 = t2 - 1;
		Float_t b3 = t3 * t3 * t3 / 6;

		// calculate the curve point
		return
			control_pts[k - 3] * b0 +
			control_pts[k - 2] * b1 +
			control_pts[k - 1] * b2 +
			control_pts[k - 0] * b3;
	}

	template<typename Float_t>
	nv::vec2<Float_t> HCurve::df(Float_t t, unsigned int order) const {
		assert(nControlPts() >= degree() + 1);
		assert(t >= tMin());
		assert(t < tMax());

		switch (order) {
		case 0:
			return at(t);
		case 1:
			return df1(t);
		case 2:
			return df2(t);
		default:
			throw "Not implemented.";
		}
	}

	template<typename Float_t>
	inline nv::vec2<Float_t> HCurve::df1(Float_t t) const {
		int k = t;
		Float_t t0 = t - (k - degree());
		Float_t t0_2 = t0 * t0;
		Float_t db0 = -(t0 - 4) * (t0 - 4) / 2;
		Float_t db1 = (9 * t0_2 - 66 * t0 + 117) / 6;
		Float_t db2 = (-9 * t0_2 + 60 * t0 - 96) / 6;
		Float_t db3 = (t0 - 3) * (t0 - 3) / 2;

		return
			control_pts[k - 3] * db0 +
			control_pts[k - 2] * db1 +
			control_pts[k - 1] * db2 +
			control_pts[k - 0] * db3;
	}

	template<typename Float_t>
	inline nv::vec2<Float_t> HCurve::df2(Float_t t) const {
		int k = t;
		Float_t t0 = t - (k - degree());
		Float_t d2_b0 = 4 - t0;
		Float_t d2_b1 = 3 * t0 - 11;
		Float_t d2_b2 = -3 * t0 + 10;
		Float_t d2_b3 = t0 - 3; 

		return
			control_pts[k - 3] * d2_b0 +
			control_pts[k - 2] * d2_b1 +
			control_pts[k - 1] * d2_b2 +
			control_pts[k - 0] * d2_b3;
	}

	template<typename Float_t>
	nv::vec2<Float_t> HCurve::tangent(Float_t t) const {
		assert(nControlPts() >= degree() + 1);
		assert(t >= tMin());
		assert(t < tMax());

		return nv::normalize(df1(t));
	}

	template<typename Float_t>
	nv::vec2<Float_t> HCurve::normal(Float_t t) const {
		assert(nControlPts() >= degree() + 1);
		assert(t >= tMin());
		assert(t < tMax());

		vec_t tgt = tangent(t);
		vec_t d2_p = df2(t);
		int sign = (tgt[0] * d2_p[1] - tgt[1] * d2_p[0]) > 0; // sign of curvature
		if (tgt.x * sign > 0) return vec_t(-tgt.y, tgt.x);
		return vec_t(tgt.y, -tgt.x);
	}

	template<typename Float_t>
	Float_t HCurve::curvature(Float_t t) const {
		assert(nControlPts() >= degree() + 1);
		assert(t >= tMin());
		assert(t < tMax());

		vec_t d1_p = df1(t);
		vec_t d2_p = df2(t);
		Float_t nmt = abs(d1_p[0] * d2_p[1] - d1_p[1] * d2_p[0]);
		Float_t dmt = nv::length(d1_p);
		dmt = dmt * dmt * dmt;
		return nmt / dmt;
	}

	template<typename Float_t>
	void HCurve::copyToDevice(BSplineCurve<Float_t, 3, DeviceType::GPU>& d_curve) const {
#if enable_cuda
		d_curve.setControlPts(this->control_pts.data(), nControlPts(), DeviceType::CPU);
#endif
	}

	template class BSplineCurve<float, 3, DeviceType::CPU>;
	template class BSplineCurve<double, 3, DeviceType::CPU>;
}